/*
 * MODBUS Slave Library: A portable MODBUS slave for MODBUS ASCII/RTU/TCP.
 * Copyright (c) 2008 Christian Walter <cwalter@embedded-solutions.at>
 * Copyright (c) 2020 Embedded Experts GmbH <modbus@embedded-experts.at>
 *
 * All rights reserved. Use of this file is subject to license terms.
 */

/* ----------------------- System includes ----------------------------------*/

#include "mbs.h"
#include "mbport.h"
#include "mbsfunctions.h"
#include "mbsiconfig.h"
#include "mbutils.h"

#include "mbsascii.h"
#include "mbsrtu.h"
#include "mbssptcp.h"

/* ----------------------- Defines ------------------------------------------*/
#define IDX_INVALID ( 255 )

/*! \brief Calculate the required number of internal states required from
 *  the number of enabled serial and TCP instances.
 * \ingroup mbs_internal
 * \internal
 */
#define MBS_MAX_HANDLES                     \
    ( (MBS_SERIAL_ASCII_MAX_INSTANCES) + \
      (MBS_SERIAL_RTU_MAX_INSTANCES) +     \
      (MBS_SERIAL_TCP_MAX_INSTANCES))

/* ----------------------- Type definitions ---------------------------------*/

/* ----------------------- Static functions ---------------------------------*/
STATIC void vMBSResetHdl( xMBSInternalHandle *pxIntHdl );

/* ----------------------- Static variables ---------------------------------*/
STATIC BOOL               bIsInitalized = FALSE;
STATIC xMBSInternalHandle xMBSInternalHdl[MBS_MAX_HANDLES];

STATIC const struct
{
    const UBYTE                   ubFunctionCode;
    const peMBSStandardFunctionCB peFunctionCB;
} arxMBSDefaultHandlers[] = {
    { MBS_FUNCCODE_READ_INPUT_REGISTERS, eMBSFuncReadInputRegister },
    { MBS_FUNCCODE_READ_HOLDING_REGISTERS, eMBSFuncReadHoldingRegister },
    { MBS_FUNCCODE_WRITE_SINGLE_REGISTER, eMBSFuncWriteSingleRegister },
    { MBS_FUNCCODE_WRITE_MULTIPLE_REGISTERS, eMBSFuncWriteMultipleHoldingRegister },
    { MBS_FUNC_READ_DISCRETE_INPUTS, eMBSFuncReadDiscreteInputs },
    { MBS_FUNC_READ_COILS, eMBSFuncReadCoils },
    { MBS_FUNC_WRITE_SINGLE_COIL, eMBSFuncWriteSingleCoil },
    { MBS_FUNC_WRITE_MULTIPLE_COILS, eMBSFuncWriteMultipleCoils },
};

/* ----------------------- Static functions ---------------------------------*/
STATIC xMBSInternalHandle *pxMBSGetNewHdl( void );
STATIC eMBErrorCode eMBSReleaseHdl( xMBSInternalHandle *pxIntHdl );

/* ----------------------- Start implementation -----------------------------*/

BOOL bMBSIsHdlValid( const xMBSInternalHandle *pxIntHdl )
{
    return MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) ? TRUE : FALSE;
}

STATIC void vMBSResetHdl( xMBSInternalHandle *pxIntHdl )
{
    pxIntHdl->eSlaveState = MBS_STATE_NONE;
    pxIntHdl->xFrameHdl = MBS_FRAME_HANDLE_INVALID;
    pxIntHdl->ubSlaveAddress = MB_SER_SLAVE_ADDR_MIN;
    pxIntHdl->bIsSerialDevice = FALSE;
    pxIntHdl->ubIdx = IDX_INVALID;
    pxIntHdl->pubFrameMBPDUBuffer = NULL;
    pxIntHdl->usFrameMBPDULength = 0;
    pxIntHdl->pFrameSendFN = NULL;
    pxIntHdl->pFrameReciveFN = NULL;
    pxIntHdl->pFrameCloseFN = NULL;
    pxIntHdl->xMBSRegCB.peMBSRegInputCB = NULL;
    pxIntHdl->xMBSRegCB.peMBSRegHoldingCB = NULL;
    pxIntHdl->xMBSRegCB.peMBSDiscInputCB = NULL;
    pxIntHdl->xMBSRegCB.peMBSCoilsCB = NULL;
}

STATIC xMBSInternalHandle *pxMBSGetNewHdl( void )
{
    eMBErrorCode        eStatus = MB_ENORES;
    xMBSInternalHandle *pxIntHdl = NULL;
    UBYTE               ubIdx;

    MBP_ENTER_CRITICAL_SECTION();
    if( !bIsInitalized )
    {
        for( ubIdx = 0; ubIdx < ( UBYTE )MB_UTILS_NARRSIZE( xMBSInternalHdl ); ubIdx++ )
        {
            vMBSResetHdl( &xMBSInternalHdl[ubIdx] );
        }
        bIsInitalized = TRUE;
    }
    for( ubIdx = 0; ubIdx < ( UBYTE )MB_UTILS_NARRSIZE( xMBSInternalHdl ); ubIdx++ )
    {
        if( IDX_INVALID == xMBSInternalHdl[ubIdx].ubIdx )
        {
            pxIntHdl = &xMBSInternalHdl[ubIdx];
            pxIntHdl->ubIdx = ubIdx;
            break;
        }
    }
    if( MB_ENOERR != eStatus )
    {
        if( NULL != pxIntHdl )
        {
            eMBSReleaseHdl( pxIntHdl );
        }
    }
    MBP_EXIT_CRITICAL_SECTION();
    return MB_ENOERR == eStatus ? pxIntHdl : NULL;
}

#if MBS_TEST_INSTANCES == 0
STATIC
#endif
eMBErrorCode eMBSReleaseHdl( xMBSInternalHandle *pxIntHdl )
{
    eMBErrorCode eStatus = MB_EINVAL;

    MBP_ENTER_CRITICAL_SECTION();
    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        if( NULL != pxIntHdl->pFrameCloseFN )
        {
            if( MB_ENOERR != ( eStatus = pxIntHdl->pFrameCloseFN( pxIntHdl ) ) )
            {
                /* We must not free the event handle since we could
                 * not close the frame instance.
                 */
            }
            else
            {
                vMBSResetHdl( pxIntHdl );
                eStatus = MB_ENOERR;
            }
        }
        /* If no frame handle has been attached we can only do a
         * partial cleanup.
         */
        else
        {
            vMBSResetHdl( pxIntHdl );
            eStatus = MB_ENOERR;
        }
    }
    MBP_EXIT_CRITICAL_SECTION();

    return eStatus;
}

eMBErrorCode eMBSClose( xMBSHandle xHdl )
{
    eMBErrorCode        eStatus = MB_EINVAL;
    xMBSInternalHandle *pxIntHdl = xHdl;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        MBP_ENTER_CRITICAL_SECTION();
        eStatus = eMBSReleaseHdl( pxIntHdl );
        MBP_EXIT_CRITICAL_SECTION();
    }
    return eStatus;
}

eMBErrorCode eMBSRegisterInputCB( xMBSHandle xHdl, peMBSRegisterInputCB peRegInputCB )
{
    eMBErrorCode        eStatus = MB_EINVAL;
    xMBSInternalHandle *pxIntHdl = xHdl;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        pxIntHdl->xMBSRegCB.peMBSRegInputCB = peRegInputCB;
        eStatus = MB_ENOERR;
    }
    return eStatus;
}

eMBErrorCode eMBSRegisterHoldingCB( xMBSHandle xHdl, peMBSRegisterHoldingCB peRegHoldingCB )
{
    eMBErrorCode        eStatus = MB_EINVAL;
    xMBSInternalHandle *pxIntHdl = xHdl;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        pxIntHdl->xMBSRegCB.peMBSRegHoldingCB = peRegHoldingCB;
        eStatus = MB_ENOERR;
    }
    return eStatus;
}

eMBErrorCode eMBSRegisterDiscreteCB( xMBSHandle xHdl, peMBSDiscreteInputCB peMBSDiscInputCB )
{
    eMBErrorCode        eStatus = MB_EINVAL;
    xMBSInternalHandle *pxIntHdl = xHdl;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        pxIntHdl->xMBSRegCB.peMBSDiscInputCB = peMBSDiscInputCB;
        eStatus = MB_ENOERR;
    }
    return eStatus;
}

eMBErrorCode eMBSRegisterCoilCB( xMBSHandle xHdl, peMBSCoilCB peMBSCoilsCB )
{
    eMBErrorCode        eStatus = MB_EINVAL;
    xMBSInternalHandle *pxIntHdl = xHdl;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        pxIntHdl->xMBSRegCB.peMBSCoilsCB = peMBSCoilsCB;
        eStatus = MB_ENOERR;
    }
    return eStatus;
}

eMBErrorCode eMBSPoll( xMBSHandle xHdl )
{
    eMBErrorCode        eStatus = MB_ENOERR, eStatus2;
    eMBException        eEXResponse;
    xMBSInternalHandle *pxIntHdl = xHdl;
    UBYTE               ubSlaveAddress;
    UBYTE               ubIdx;
    UBYTE               ubFunctionCode;
    BOOL                bIsBroadcast = FALSE;

    if( MB_IS_VALID_HDL( pxIntHdl, xMBSInternalHdl ) )
    {
        switch( pxIntHdl->eSlaveState )
        {
            case MBS_STATE_NONE:
                /* Note: Check if we still need the RTU startup code. */
                pxIntHdl->eSlaveState = MBS_STATE_WAITING;
                break;

            case MBS_STATE_WAITING:
                /* Wait for new MODBUS requests from a master. */
                eStatus2 = pxIntHdl->pFrameReciveFN( pxIntHdl, &ubSlaveAddress, &( pxIntHdl->usFrameMBPDULength ) );
                switch( eStatus2 )
                {
                    case MB_ENOERR:
                        /* MBS_ANY_ADDR (0xFF) is used in TCP mode for addressing
                            * the slave. In serial mode we check if the frame is for
                            * us.
                            */
                        if( ubSlaveAddress == pxIntHdl->ubSlaveAddress )
                        {
                            pxIntHdl->eSlaveState = MBS_STATE_EXECUTE;
                        }
                        else if( MB_SER_BROADCAST_ADDR == ubSlaveAddress )
                        {
                            pxIntHdl->eSlaveState = MBS_STATE_EXECUTE_BROADCAST;
                        }
                        else
                        {
                            /* Do a dummy transmission to reenable the receiver. */
                            if( MB_ENOERR != pxIntHdl->pFrameSendFN( pxIntHdl, 0 ) )
                            {
                                pxIntHdl->eSlaveState = MBS_STATE_ERROR;
                            }
                        }
                        break;

                        /* This frame was garbage. Do nothing. */
                    case MB_EIO:
                        /* Do a dummy transmission to reenable the receiver. */
                        if( MB_ENOERR != pxIntHdl->pFrameSendFN( pxIntHdl, 0 ) )
                        {
                            pxIntHdl->eSlaveState = MBS_STATE_ERROR;
                        }
                        /* Simply ignore this frame. No need to signal an error
                            * to the caller.
                            */
                        break;

                    default:
                        /* Transistion to error state. */
                        pxIntHdl->eSlaveState = MBS_STATE_ERROR;
                        break;
                }
                break;

                /* Fallthrough to next case which takes care of handling the
                    * function.
                    */
            case MBS_STATE_EXECUTE_BROADCAST:
                bIsBroadcast = TRUE;
                /*lint -fallthrough */
            case MBS_STATE_EXECUTE:
                /* The default is that we assume that no such function is
                    * available.
                    */
                eEXResponse = MB_PDU_EX_ILLEGAL_FUNCTION;
                ubFunctionCode = pxIntHdl->pubFrameMBPDUBuffer[MB_PDU_FUNC_OFF];
                /* Check if frame has not already been handled by the custom
                    * function handlers.
                    */
                if( MB_PDU_EX_ILLEGAL_FUNCTION == eEXResponse )
                {
                    for( ubIdx = 0; ubIdx < ( UBYTE )MB_UTILS_NARRSIZE( arxMBSDefaultHandlers ); ubIdx++ )
                    {
                        if( ubFunctionCode == arxMBSDefaultHandlers[ubIdx].ubFunctionCode )
                        {
                            eEXResponse = arxMBSDefaultHandlers[ubIdx].peFunctionCB(
                                pxIntHdl->pubFrameMBPDUBuffer, &( pxIntHdl->usFrameMBPDULength ),
                                &( pxIntHdl->xMBSRegCB ) );
                            break;
                        }
                    }
                }

                if( !bIsBroadcast )
                {
                    /* In case of an exception we must build an exception frame. */
                    if( MB_PDU_EX_NONE != eEXResponse )
                    {
                        pxIntHdl->usFrameMBPDULength = 0;
                        pxIntHdl->pubFrameMBPDUBuffer[pxIntHdl->usFrameMBPDULength] =
                            ( UBYTE )( ubFunctionCode | 0x80 );
                        pxIntHdl->usFrameMBPDULength++;
                        pxIntHdl->pubFrameMBPDUBuffer[pxIntHdl->usFrameMBPDULength] = ( UBYTE )eEXResponse;
                        pxIntHdl->usFrameMBPDULength++;
                    }
                    pxIntHdl->eSlaveState = MBS_STATE_SEND;
                }
                else
                {
                    /* Reenable receiver after broadcast. */
                    if( MB_ENOERR != pxIntHdl->pFrameSendFN( pxIntHdl, 0 ) )
                    {
                        pxIntHdl->eSlaveState = MBS_STATE_ERROR;
                    }
                    else
                    {
                        pxIntHdl->eSlaveState = MBS_STATE_WAITING;
                    }
                }
                break;

            case MBS_STATE_SEND:
                
                eStatus2 = pxIntHdl->pFrameSendFN( pxIntHdl, pxIntHdl->usFrameMBPDULength );
                switch( eStatus2 )
                {
                    case MB_ENOERR:
                        pxIntHdl->eSlaveState = MBS_STATE_WAITING;
                        break;

                    case MB_EIO:
                        pxIntHdl->eSlaveState = MBS_STATE_WAITING;
                        break;

                    case MB_EPORTERR:
                    default:
                        pxIntHdl->eSlaveState = MBS_STATE_ERROR;
                        break;
                }
                break;

                /* The stack is broken and needs to be restarted. */
            case MBS_STATE_ERROR:
                eStatus = MB_EILLSTATE;
                break;
        }
    }
    else
    {
        eStatus = MB_EINVAL;
    }
    return eStatus;
}

eMBErrorCode eMBSSerialInit( xMBSHandle *pxHdl, eMBSerialMode eMode, UBYTE ubSlaveAddress, pusSendDatas pMBSFrameSendFN, pusReciveDatas pMBSFrameRecvFN)
{
    xMBSInternalHandle *pxMBSNewIntHdl;
    eMBErrorCode        eStatus = MB_EINVAL, eStatus2;

    if( NULL != pxHdl )
    {
        if( NULL == ( pxMBSNewIntHdl = pxMBSGetNewHdl() ) )
        {
            eStatus = MB_ENORES;
        }
        else
        {
            switch( eMode )
            {
                case MB_ASCII:
                    break;

                case MB_RTU:
                    eStatus = eMBSSerialRTUInit( pxMBSNewIntHdl, pMBSFrameSendFN, pMBSFrameRecvFN);
                    break;

                default:
                    eStatus = MB_EINVAL;
                    break;
            }
        }

        if( eStatus != MB_ENOERR )
        {
            if( NULL != pxMBSNewIntHdl )
            {
                if( MB_ENOERR != ( eStatus2 = eMBSReleaseHdl( pxMBSNewIntHdl ) ) )
                {
                    eStatus = eStatus2;
                }
            }
            *pxHdl = NULL;
        }
        else
        {
            pxMBSNewIntHdl->ubSlaveAddress = ubSlaveAddress;
            pxMBSNewIntHdl->bIsSerialDevice = TRUE;

            *pxHdl = pxMBSNewIntHdl;
        }
    }

    return eStatus;
}
